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ABSTRACT 

Virtual reality can enable a robot user to off line generate and test in a virtual enviro nmen t a sequence of 
operations to be executed by the robot in an assembly cell. Virtual models of objects are to be correlated 
to the real entities they represent by means of a suitable transformation. A solution to the correlation 
problem, which is basically a problem of 3-dimensional adjusting, has been found exploiting the surface 
matching theory. An iterative algorithm has been developed, which matches the geometric surface 
representing the shape of the virtual model of an object, with a set of points measured on the surface in 
the real world. A peculiar feature of the algorithm is to work also if there is no one-to-one 
correspondence between the measured points and those representing the surface model. Furthermore, the 
problem of avoiding convergence to local minima is solved, by defining a starting set of states ensuring 
convergence to the global minimum. The developed algorithm has been tested by simulation. Finally, 
this paper proposes a specific application, i.e. correlating a robotized cell, equipped for biomedical use' 
with its virtual representation. 


1. INTRODUCTION 

The most recent developments of computer graphics 
allow to create high quality virtual representations of 
real entities. 

Such virtual images provide a useful representation of 
the real world only if a transformation is defined, 
correlating the virtual models to the real world. 

This paper investigates the problem of finding a 
correlation between a real entity and its virtual model. 
Such a problem is often encountered in many 
specialists fields (i.e. in biomedical applications). 
Virtual reality, intended as the capability to represent a 
3 -dimensional environment by means of virtual models 
of the objects constituting it, is used in robotics as a 
powerful support to offline programming. 

As a matter of fact, the offline programming technique 
increases the productivity of a robotized cell, by 
avoiding that the robot be stopped for a long time, in 
order to be reprogrammed by means of teach-in 
operations. 

Recent developments of CAD systems allow to build 
robotic simulators that can associate the typical CAD 
data structures with high quality images. These 
features enable the user of the simulator to off line 
generate operating sequences representing the 
movements of the robot and to test its interactions with 


the parts inside the cell. 

These sequences, easily generated in the virtual 
environment, can be applied to the real cell only if the 
correlation between the virtual and the real cell is 
known. A recent paper [1] describes a procedure of 2- 
dimensional adjusting, that finds this correlation in the 
case of an object lying on a working table. This 
procedure has been tested and applied to the field of 
automatic assembling. An infrared sensor is used to 
detect the position of the object. 

This paper proposes a more general solution of the 
adjusting problem, i.e. a solution in the 3-dimensional 
case. The approach to the problem is rather different: a 
laser sensor has been used instead of the infrared 
sensor, so that analog distance measurement in a 
longer range are now possible, and the developed 
algorithm is based on the surface matching theory 
instead of simpler 2-dimensional geometric 
considerations. 

The paper is organized in three main sections: the first 
section contains an overview of the surface matching 
theoiy, the second one proposes an ad hoc algorithm to 
solve the surface matching problem in the 3- 
dimensional case and some tests to validate it; finall y 
a combined robotic and biomedical application is 
discussed. 
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2. THEORY 

The surface matching theory is aimed at finding a 
correlation between two different representations of the 
same surface. 

In most applications one of these representations is 
obtained scanning the surface of a real object by a 
sensor, so that all the data are referred to the sensor's 
reference frame, whereas the other representation is a 
virtual model of the same surface, stored in the 
memory of a computer. 

A two-level definition of the matching problem can be 
given, depending on how the surfaces are represented: 

1) given two sets of points, representing the same 
surface in two different reference frames, find the rigid 
transformation (expressed by a rototranslation matrix) 
mapping one set of points into the other. Such a 
transformation has the following characteristics: 

a) must be optimal with respect to some criterion 
(e.g. minimize the maximum or the mean squared 
difference of the distances between corresponding 
points); 

b) must work for sets of points with different 
dimensions; 

c) must work also if points in one set do not 
correspond exactly to points in the other; 

d) must work also if the points of the data set are 
corrupted by noise. 

2) given a real surface and its virtual model, a set of 
points is obtained by sampling the real surface. Find 
the rigid transformation rototranslating the modeled 
surface in order to minimize its "distance" from the set 
of points. This case may be seen as a generalization of 
the previous one: therefore, the transformation must 
have the same characteristics a) thru d). 


2.1 State of the art 

The following are some more formal remarks on the 
matching problem. 

Be X a set of points and (R,t ) a rototranslation 
defined by a rotation matrix R and a translation vector 
t , let us call P the set of points obtained applying the 
rototranslation (R, t ) to the set X. It is simple to obtain 
the rototranslation matrix (Rj ) starting from the 
knowledge of X and P, if the one-to-one 
correspondence of the points of the two sets is known. 
The problem of determining the transformation (Rj ) 
becomes more difficult if the points of one set are 
affected by noise, in the sense that the relationship 

*, = R 'Pi+t (1) 


above equation x, and p i are the coordinates of the i'-th 
point (i = I...N) of the sets X and P respectively. 

In this case the problem becomes a minimization 
problem: it is required to found the matrix R and the 
vector 7 that minimize the sum of the errors 

e^Xt-R.pi-7 ( 2 ) 

The general matching problem does not require any 
one-to-one correspondence between the points of X and 
the points of P. This implies that no rototranslation 
exists, which maps exactly every point ofX into a point 
of P even in the case of zero noise. 

Some authors have investigated the matching problem, 
applying their algorithms to specific cases. 

If the one-to-one correspondence is known the 
matching problem can be solved using the methods 
proposed by Horn and Haralick. 

Horn [2] proposes a very simple method to determine 
the rototranslation matrix in the 2-dimensional case 
(i.e. when all points of each set lie in the same plane). 
A 3x4 rototranslation matrix is obtained finding the 
3x3 rotation matrix first and then the 3x1 translation 
vector. 

The algorithm proceeds as follows: given two sets of 
points X and P, with the same dimension N, 
considering three not aligned points of the P set, and 
the corresponding ones of the X set, build an adequate 
reference frame for each set of points, according to the 
following rules: 

a) the origin be the first point; 

b) the X axis be the line connecting the first and the 
second point; 

c) the Y axis be the line, coplanar with the three 
points, and normal to the X axis; 

d) the Z axis be chosen following the right hand rule. 
Once these frames have been built, it is straightforward 
to find the 3x3 rotation matrix R between them. The 
translation vector is then found recalling that 
corresponding points in the two sets are linked by the 
following relationship: 

x i = R-p i +7 ( 3 ) 

The algorithm yields an exact result only if the points 
are not affected by noise; otherwise, it is not possible to 
find the translation satisfying the above equation 
exactly, but the transformation correlating each pair of 
corresponding points is affected by an error: 

e,. =x,-R .p f -7 (4) 


does not hold for all pairs of points of X and P. In the 
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Thus, the problem becomes: find the rigid 

transformation that minimizes the sum of the squared 
errors, due to the transformation of all the points of the 



Pset: 

s 2 = ZWf (5) 

7=1 

Another approach to solve the problem in the 2- 
dimensional case has been developed by Haralick [3j. 
His method finds the 3x3 rotation matrix considering 
all the points in the set simultaneously (whereas the 
Horn's technique considered only three points at a 
time). If N = N x = N p is the number of points in each 

of the two sets, the mean squared error e 2 to be 
minimized is now: 

e2 = X w i ’ pi - ( R * A + Of (6) 

7=1 

where the weights w, should meet the conditions: 

N 

w ' s 0 ’X w ' =1 (7) 

7=1 

By choosing the weights in a convenient way, the 
method is made robust and stable. A good rule to 
choose the weights is to associate a greater weight to 
those points with lower squared error. The steps to 
build the Haralick estimator are the followings: 

a) starting from an initial value for the rotation matrix 
R and the translation vector / , determine the errors 
e 2 for each pair of corresponding points; 

b) the weights can now be chosen, using the Tukey 
function, applied to the errors e 2 : 


transformation is described by a 7-dimensional vector 
instead of a 3x4 matrix. This method reveals itself 
accurate and computationally efficient; furthermore, it 
works also if there is no one-to-one correspondence 
between the two sets of points representing the 
surfaces. 

3. DESCRIPTION OF THE ALGORITHM 

An algorithm has been developed, which matches the 
descriptor of a surface representing a virtual model 
(e.g. a set of points gotten from the model), with a 
surface descriptor extracted from the corresponding 
real object (e.g. a set of points measured on the surface 
of the object in the real world). This algorithm is a 
modification and an evolution of the Closest Point 
Algorithm proposed by Besl [4]. An important feature 
of this algorithm is that it works also if there is no one- 
to-one correspondence between the points of the X and 
the P sets. 

Some preliminary definitions will now be given. Let us 
call X the model set, i.e. a set of points representing the 
modeled surface and P the data set, i.e. a set of points 
representing the real surface (e.g. points gotten 
sampling the surface by means of a sensor). Both sets 
have the same dimension N. 

The matching problem is solved finding: 

- a correspondence K between the two sets of points; 

- a rotation matrix R and a translation vector t linking 
each point of the model with the corresponding data 
point, that minimizes the sum of the squared errors (5). 
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1 - 


(c-Sf 


otherwise 


( 8 ) 


where c and S are parameters of the Tukey function; 
namely, c is chosen between 6 and 12, and S is the 
median of the absolute deviation of the errors e 2 ; 

c) solve the minimization problem using the weights 
that have been computed in the previous step; in this 
way new values for R and t are obtained; 

d) iterate the steps b) and c) until the global error s 2 
decreases below a fixed threshold. 

The Haralick technique can be extended to the 3- 
dimensional case (see [3]). 

Another solution to the surface matching problem in 
the 3-dimensional case is given by Besl. He proposes a 
method, based on the Iterative Closest Point (ICP) 
algorithm, to match two 3-dimensional surfaces. This 
technique, described in [4], utilizes quaternions to 
represent rotations; thus, the rototranslation 


Two kinds of errors that are implicitly included in (5) 
are: the measurement errors (affecting the data set P), 
and the errors in the model (affecting the model set JO- 
The latter are due to the fact that the virtual surface is 
not an exact model of the real surface; the former in 
most cases may be neglected. However, both these 
errors cannot be minimized by the matching algorithm. 

The matching problem can be classified into: 

- global matching 

- local matching. 

In the first case, there is a biunivocal correspondence 
between all the points of the model and all the data 
points, because the data represent the whole surface. It 
is required to determine R and t that minimize the 
function G : 

G(X,/ > ) = jmo||X-(R-P+?)|| (9) 


In the case of local matching, the data represent only a 
part of the surface (thus, the dimension of P is 
necessarily smaller than the dimension of X). It is 
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( 16 ) 


required to determine not only R and t , but also d(p,T)= min dtp,^) 

which part Z of the model X minimizes the function L : 


L(X,P) = min |m^||Z-(R*P+F 
- min G(Z,P) 

ZqX 


( 10 ) 


Before going further into the description of the 
algorithm, let us define which type of distance between 
geometric entities is assumed in the algorithm. 

The distance between two points r x - (x^y^Zi) and 
r 2 - ( x 2 ’^ 2 ’ 2 2 ) * s assumed to be the euclidean 
distance: 


Both a curve and a parametric surface are described by 
a relationship r(ti), where: 

u - u e A c SR 1 for parametric curves; 

u = (u, v) e A c $R 2 for parametric surfaces. 

The domains is a segment if r{u) is a curve; it is a 
closed region in the plane if r(u) is a surface. 

The distance between a point p and the parametric 
entity E is defined as: 


= i/U -Xif+iyi -yif -^) 2 


(ii) 


Letyf = {5, } be a set of points, (i-L.Ay, where N a is 
the number of points, the distance between a point p 
and the set A is defined as: 


d(p>E) = min d(p,r(u)) (17) 

r{u)eE 

Then, if P = {pJ is a set of parametric entities, 
(i=l..N e ) y where N e is the number of entities, the 
distance between a point p and the set F is defined as: 

d(p,F)= min d{pX) (18) 


d (P, A )= min </(p,5 f ) (12) 

Let / be a segment connecting and r 2 , the distance 
between a point p and the segment / is: 

d{p,l)= min +v-r 2 -p\\ (13) 

K+V=l 

where u e[0,l] and v e[0,l]. 

Then, if L = } is a set of segments, (i=l.. JV ; ), where 

N { is the number of segments, the distance between a 
point p and the set L is defined as: 

d(p,L)~ min dtp, If) (14) 

Let t be a triangle whose vertices are /[ , r 2 and r 2 ; the 
distance between a point p and the triangle t is defined 
as: 


These mathematical concepts will be useful in the 
following description of the global surface matching 
algorithm. The subscript k is used to indicate the 
quantities involved in the &-th iteration of the 
algorithm. 

Let P = {pi } and X = { x, } be the two sets of points to 
be matched. 

If P and X have the same dimension (N x = Ay, the 

matching problem can be solved using the above 
described Haralick method, setting the initial 
conditions: R 0 = / 3 , ? 0 = 0 (so that P 0 = P). We define 
the 0 operator as the function that performs the 
registration between P andX, i.e. computes the optimal 
rotation matrix that matches P and X. So, for each 
iteration new values for R and t are obtained by 
applying the Q operator as follows: 

(R k J t ,d„) = e(P k ,X) k> 1 (19) 


d(p,t) = min +v*r 2 +w*r 3 -pll (15) 

U^V4-W=l 11 " 

where u e [0, l] , v e [0, l] and w e[0,l] . 

Then, if T = {/,-} is a set of triangles, (i=l..N t ), where 
N t is the number of triangles, the distance between a 
point p and the set T is defined as: 


where d k is the mean squared error given by (5). The 
value of P k is obtained applying the rotation R*., and 
the translation t k _ x to the whole set P k _ x , summarized 
by the formula: 

Pk ~ Rjfc-iA*— i + h-\ (20) 

The iterations stop when the absolute value of the 
difference between two consecutive mean squared 


79 



errors is lower than a fixed positive threshold t : 

K-4w|<r (2D 

Now, let us consider the more general problem of 
matching two sets of points with different dimensions. 
To solve this problem an iterative algorithm of the 
"closest point" type is used. 

Let us suppose that the dimension of the set of the 
model points is greater than that of the set of data 
points ( N x > N p ), and let us call Y k the set of the N p 

points of X which are the closest to the points of P (i.e. 
are the "best correspondent points") in the fc-th 
iteration; this defines, for each iteration, a new 
correspondence K. Let us call C the operator 
performing this computation: 

Y k =C(P k ,X) (22) 

Now the optimal rotation matrix R and the optimal 
translation vector t can be computed using the above 
defined 0 operator applied to the Y k set: 

(R k J k ,d k ) = Q(P k ,Y k ) (23) 

The rototranslation (R*,4) thus computed is then 
applied to all the points of X y obtaining a new set ?k+\ 
which is closer to theX set (see [4] for a proof). 

The C operator is now applied to the new set in 
order to determine the new set y k+ i of points closest to 
X. 

The loop is iterated until the difference between the 
mean squared errors in two consecutive iterations is 
lower than a fixed positive threshold t . 

The convergence of this algorithm to a local minimum 
has been demonstrated [4]. However, the convergence 
to the global minimum is not assured in the general 
case. A way to make the algorithm converge to the 
smallest local minimum is to start the algorithm 
choosing R 0 in an adequate set of initial rotations, 
called "states", instead of choosing R 0 = I 3 . Besl [4] 
has investigated how to find a suitable set of initial 
states. 

This algorithm can be used also to solve the local 
matching problem; in this case, it is necessary to 
introduce a set of initial translations in addition to the 
set of initial rotations, in order to avoid convergence to 
a local minimum. 

The algorithm has been tested for both global and local 
matching. The set X of points of the model is made of 
55 points sampled on the surface of an ellipsoid. Four 
tests have been made using different P sets. 


The first test applies the algorithm to the case of a 
global matching in ideal conditions (i.e. there is no 
noise). Of course, the algorithm converges exactly. 

In the second test a gaussian noise has been added, to 
account for errors in the model and in the 
measurement. The error has zero mean and its 
variance is one twentieth of the maximum absolute 
value of the coordinates of the data points. 

The algorithm converges after testing four initial 
rotations. 

Fig. 1 and 2 show the sets before and after the 
algorithm has been run. 



Figure 1 - Sets of points to be matched 



Figure 2 - Result of matching 

The third test is a local matching between 24 and 55 
points, with additional noise. The results are shown in 
Fig. 3 and 4. 



Figure 3 - Sets of points to be matched 
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Figure 4 - Result of matching 

Finally, the algorithm has been run to match two sets 
of 15 and 55 points, with a gaussian noise with zero 
mean and variance three times greater than the 
previous value. The results are shown in Fig. 5 and 6. 



Figure 5 - Sets of points to be matched 



Figure 6 - Result of matching 

This technique to match two sets of points can be 
extended to the case when the model is not represented 
by a set of points but by a surface. This can be 
convenient when the surface is expressed in an 
analytical form (e.g. if the model is built using a CAD 
system, or if the analytical form of the surface is 
known). To run the algorithm, the points of the model 
set are chosen to be the points on the surface which are 
the nearest to each point of the data set. 


In this case the algorithm requires a method to 
compute the distance between a point and a parametric 
surface. To compute the distance between a point and a 
parametric surface, the latter can be approximated by a 
set of triangles, whose vertices lie on the surface. The 
shorter the edges of the triangles are, the better the 
approximation is. 

Therefore, the problem of computing the distance 
between a point and a parametric surface is turned into 
the problem of computing the distance between a point 
and a set of triangles, which has been defined above. 
The next three figures represent the matching between 
the set of the data points and the surface expressed in 
an analytical foim. 

ICC*, 
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Figure 7 - The surface and the set of points to be matched 


j 



Figure 8 - An intermediate result of matching 


too-, 

scJ 



Figure 9 - Result of matching 
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4. APPLICATION TO ROBOTICS 


A special system has been developed, whose structure 
is made of two levels: a real workcell equipped with an 
ASEA type IRB2000 industrial 6 d.o.f. robot and a 
simulation environment based on a prototype of a 
robotic simulator able to represent exactly the real cell. 
This system was initially conceived for automatic 
assembling purposes; therefore, it has been equipped 
with a sliding worktable and a set of automatically 
changeable tools (e.g. grippers, screwers, IR and laser 
sensors). 

The flexibility of the system is such that it can be used 
also in a rather different research field as robotics 
applied to biomedics. While in the automatic 
assembling field a 2-dimensional adjusting procedure 
turned out to be sufficient in most cases, a more 
sophisticated 3-dimensional adjusting procedure is 
necessary to find the real position of the object, being 
this now a patient. 

A major difference from the mechanical case, where all 
parts are modeled by means of a CAD system which is 
already integrated with the simulator, is that in the 
biomedical case the virtual models of the part of 
interest of the patient are obtained by correlating 
images gotten by different diagnostical exams (e.g. 
NMR, CT, DA). (See [6], [7], [10], [14], [15], [19], 
[20] for reference). 

The proposed application refers to a skull represented 
by a dumb in the real world, and by a virtual model 
reconstructed from diagnostical images in the 
simulation environment. 

A correlation is then to be established between the 
virtual model and the patient's skull, by matching the 
virtual surface and a set of points taken on the real 
skull using a laser sensor mounted on the end effector 
of the robot. In this way it is also possible to make a 
further correlation between the skull and the robot, so 
that to establish a full correlation between the virtual 
reality and the real world. 

An operating procedure on this skull can then be 
defined in the simulation environment, and the 3- 
dimensional surface matching based adjusting 
procedure can be used to translate the operational 
sequence into a code suitable for the real cell, and 
executable by the robot. The translation procedure can 
be done automatically, since the translator already 
developed for the robotized assembling system can be 
employed. 


5. CONCLUSIONS 

The problem of finding a correlation between a real 
entity and its virtual model has been investigated in 
this paper. Solution to this problem can provide a 


powerful tool in robotics, particularly useful for off line 
programming. 

An algorithm has been proposed, based on the surface 
matching theory, which matches the surface of a rea 
object with its virtual model. Two cases have beer 
taken into account, namely the matching between twe 
sets of points representing the real and the modeled 



Figure 10 - The real skull 



Figure 1 1 - The CAD model of the skull 



Figure 12 - The real cell 
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Figure 13 - The virtual model of the cell 


surface respectively, and the matching between a set of 
points gotten from the real surface and the virtual 
surface, expressed in an analytical form. A peculiar 
feature of the algorithm is to work also if the two sets 
of points have different dimensions, and if there is no 
one-to-one correspondence between them. Moreover, 
both the global and the local matching problems have 
been defined and a solution to them has been proposed. 
The proposed algorithm has been tested by simulation. 
Finally, a special system, composed of a robotized cell 
and a simulation environment, initially conceived for 
automatic assembling purposes, has been presented, 
and its application to the biomedical field has been 
discussed. 
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